package legoLab08;

import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;

public class Vehicle2bThreadV2 extends Behavior{
	LightSensor l1 = new LightSensor(SensorPort.S2);
	LightSensor l2 = new LightSensor(SensorPort.S3);
	
	public Vehicle2bThreadV2(String name, int LCDrow, Behavior b)
    {
    	super(name, LCDrow, b);
		
		ThreadControl left = new ThreadControl(MotorPort.C, l1, 60, 10);
		ThreadControl right = new ThreadControl(MotorPort.B, l2, 60, 10);
		
		left.start();
		right.start();
	}
	
	private class ThreadControl extends Thread{
		private MotorPort m;
		private LightSensor s;
		private int defSpeed, nrSamples, curSample;
		private int MAX_LIGHT = 2^16;
		private int MIN_LIGHT = 0; 
		private int[] samples;
		
		public ThreadControl(MotorPort motor, LightSensor sensor, int defaultSpeed, int nrSamples){
			m = motor;
			s = sensor;
			defSpeed = defaultSpeed;
			samples = new int[nrSamples];
			this.nrSamples = nrSamples;
		}
		
		@Override
		public void run() {
			while(!Button.ESCAPE.isPressed()){
				suppress();
				m.controlMotor(readVal(), 1);
				release();
			}
		}
		
		private int readVal(){
			int read = normalize(s.readNormalizedValue()); 
			
			//gem min og max
//			if( curSample < nrSamples){
//				samples[curSample] = read;
//				curSample++;
//			}
//			else{
//				for(int i = 0; i < nrSamples; i++){
//					
//				}
//			}
			if( MAX_LIGHT < read) MAX_LIGHT = read;
			if( MIN_LIGHT > read) MIN_LIGHT = read;
			
			return read;
		}
		
		private int normalize(int light){
			int output = 100 - ((light - MAX_LIGHT)*100) / (MIN_LIGHT-MAX_LIGHT);
			
			if( output < 0 ) return 0;
			if (output > 100) return 100;
			
			return output;
		}
	}
}